package org.nashua.tt151;

import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.KinectStick;
import edu.wpi.first.wpilibj.SimpleRobot;
import org.nashua.tt151.RobotMap.DualAction;
import org.nashua.tt151.RobotMap.Jaguar.Power;
import org.nashua.tt151.RobotMap.Kinect;

/**
 * This class is sample code for driving the robot using four Jaguars and the
 * Joystick class that is being read from a Logitech DualAction F310 (D mode)
 * in Operator Control and the KinectStick class in Autonomous.
 *
 * NOTE: This is using FRCSquawkSDK Version 2012.3077. When the 2013 versions
 * are released this class will be updated according.
 */
public class DriveJaguarsBuiltIn extends SimpleRobot {
    /* Hardware */
    public static Jaguar backLeft = new Jaguar(RobotMap.Jaguar.BACK_LEFT);
    public static Jaguar backRight = new Jaguar(RobotMap.Jaguar.BACK_RIGHT);
    public static Jaguar frontLeft = new Jaguar(RobotMap.Jaguar.FRONT_LEFT);
    public static Jaguar frontRight = new Jaguar(RobotMap.Jaguar.FRONT_RIGHT);
    public static Joystick joy = new Joystick(1);  // 1st Joystick Detected
    public static KinectStick leftArm = new KinectStick(1);
    public static KinectStick rightArm = new KinectStick(2);

    /* Other Variables */
    public static double power = Power.DEFAULT_POWER;  // Set to default power

    /**
     * This method is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        while (isEnabled() && isAutonomous()) {
            /* Determine Power */
            power = Power.DEFAULT_POWER; // Reset to default power
            if (leftArm.getRawButton(Kinect.Button.HEAD_LEFT))
                power = Power.CREEP_POWER; // Set to creep while head left
            if (leftArm.getRawButton(Kinect.Button.HEAD_RIGHT))
                power = Power.MAX_POWER;  // Set to max power while head right

            /*
             * Store axis positions to avoid accidentally setting the back and
             * the front Jaguars to 2 different values. The stored values are
             * multiplied by the power multiplier.
             */
            double left = leftArm.getY() * power;
            /*
             * Generally, the value of the right y axis needs to be reversed
             * since the motors are rotating in the opposite direction of the
             * left motors. Always check the robot as this is not always true.
             */
            double right = -rightArm.getY() * power;
            backLeft.set(left);
            frontLeft.set(left);
            backRight.set(right);
            frontRight.set(right);
        }
    }
    
    /**
     * This method is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        while (isEnabled() && isOperatorControl()) {
            /* Determine Power */
            power = Power.DEFAULT_POWER; // Reset to default power
            if (joy.getRawButton(DualAction.Button.LEFT_TRIGGER))
                power = Power.CREEP_POWER; // Set to creep while holding LT
            if (joy.getRawButton(DualAction.Button.RIGHT_TRIGGER))
                power = Power.MAX_POWER;  // Set to max power while holding RT

            /*
             * Store axis positions to avoid accidentally setting the back and
             * the front Jaguars to 2 different values. The stored values are
             * multiplied by the power multiplier.
             */
            // For DualAction this is the same as: double left = joy.getY();
            double left = joy.getRawAxis(DualAction.Axis.LEFT_Y) * power;
            /*
             * Generally, the value of the right y axis needs to be reversed
             * since the motors are rotating in the opposite direction of the
             * left motors. Always check the robot as this is not always true.
             */
            double right = -joy.getRawAxis(DualAction.Axis.RIGHT_Y) * power;
            backLeft.set(left);
            frontLeft.set(left);
            backRight.set(right);
            frontRight.set(right);
        }
    }
}
